/**
 * @file state.c
 * @brief 系统状态管理模块
 *        负责监控设备通信状态（遥控器、视觉、裁判系统等）、电机状态（GM6020/C620）及异常检测（超时、过温等）
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025 邱伟凡
 * 
 * @par 修改日志:
 *   2025-08-08: 初始版本，实现基础状态监控逻辑
 * 
 * @bug 待解决问题:
 *   - 部分状态恢复条件需优化
 */
//#include "iwdg.h"
#include "QWF_Ctrl.h"
/**
 * @defgroup State_Macro 状态检测宏定义
 * @brief 定义设备超时时间、异常阈值等常量
 */
#define c620_i_err_max				((int16_t)24000	)
#define c620_i_err_min				((int16_t)7000 	)
#define gm6020_i_err_max			((int16_t)24000	)
#define gm6020_i_err_min			((int16_t)7000 	)
#define gm6020_trm_err_max_1		((int16_t)90    )
#define gm6020_trm_err_max_2		((int16_t)60    )
#define gm6020_trm_err_min			((int16_t)50    )
/* 超时时间定义,包含恢复最低时间（单位ms）*/
#define lose_time_max_super_elec	((int16_t)1200 )
#define lose_time_max_RC			((int16_t)1000   )
#define lose_time_min_RC			((int16_t)500   )
#define lose_time_max_vision		((int16_t)1200 )
#define lose_time_min_vision		((int16_t)700  )
#define lose_time_max_gyro			((int16_t)550   )
#define lose_time_min_gyro			((int16_t)250   )
#define lose_time_max_gm6020		((int16_t)200   )
#define lose_time_min_gm6020		((int16_t)100   )
#define lose_time_max_c620			((int16_t)200   )
#define lose_time_min_c620			((int16_t)100   )
#define err_time_max				((int16_t)500 	)
#define lose_time_max_communicate	((int16_t)1000  )
#define lose_time_min_communicate	((int16_t)500   )
#define lose_time_max_judge	((int16_t)1000  )
#define lose_time_min_judge	((int16_t)500   )

/**
 * @brief 系统状态结构体
 *        存储各模块实时状态（在线状态、异常类型等）
 */
static __align(1) QWF_State_t QWF_State={.head.rc.judge.state=rc_lose};
QWF_State_t*qwf_state=&QWF_State;

/**
 * @brief GM6020电机状态检测宏
 * @details 检测电机在线状态、温度异常，并根据条件恢复正常状态
 * @param state_test 状态检测结构体（包含在线时间戳和当前状态）
 * @param motor 电机数据结构体（包含温度、扭矩等实时数据）
 */
#define gm6020_state_trr_func(state_test,motor)\
			do{\
				if((qwf_state->sys_tick-state_test.online.tick)>=lose_time_max_gm6020){\
					state_test.online.yes=0;\
					state_test.state=gm6020_lose;\
				}\
				else if( motor.rx_meas.trm>=gm6020_trm_err_max_1){ \
					state_test.state=gm6020_too_hot_1;}\
				else if( motor.rx_meas.trm>=gm6020_trm_err_max_2){\
					state_test.state=gm6020_too_hot_2;}\
				/*else if(motor.rx_meas.torque>=gm6020_i_err_max) {\
					state_test.state=gm6020_i_over_max;}\
				*/switch((int)state_test.state){\
					case gm6020_ok:break;\
					case gm6020_too_hot_1:{\
						if(motor.rx_meas.trm<=gm6020_trm_err_min)\
							state_test.state=gm6020_ok;\
					}break;\
					case gm6020_too_hot_2:{\
						if(motor.rx_meas.trm<=gm6020_trm_err_min)\
							state_test.state=gm6020_ok;\
					}break;\
					/*case gm6020_i_over_max:{\
						if(motor.rx_meas.torque<=gm6020_i_err_min)\
							state_test.state=gm6020_ok;\
					}break;*/\
					case gm6020_lose:{\
						if((qwf_state->sys_tick-state_test.online.tick)<=lose_time_min_gm6020){\
							state_test.online.yes=1;\
							state_test.state=gm6020_ok;\
						}\
					}break;\
				}\
			}while(0);		
/**
 * @brief C620电机状态检测宏
 * @details 检测电机在线状态，并根据条件恢复正常状态（当前仅处理失联，可扩展过流检测）
 * @param state_test 状态检测结构体
 * @param motor 电机数据结构体
 */				
#define c620_state_trr_func(state_test,motor)\
			do{\
				if((qwf_state->sys_tick-state_test.online.tick)>=lose_time_max_c620){\
					state_test.online.yes=0;\
					state_test.state=c620_lose;\
				}\
/*				else if(motor.rx_meas.torque>=c620_i_err_max) {\
					state_test.state=c620_i_over_max;\
				}*/\
				switch((int)state_test.state){\
					case c620_ok:break;\
					/*case c620_i_over_max:{\
						if(motor.rx_meas.torque<=c620_i_err_min){\
							state_test.state=c620_ok;\
							state_test.online.yes=1;\
						}\
					}break;*/\
					case c620_lose:{\
						if(qwf_state->sys_tick-state_test.online.tick<=lose_time_min_c620)\
							state_test.state=c620_ok;\
					}break;\
				}\
			}while(0);
/**
 * @brief 陀螺仪状态检测宏
 * @details 检测陀螺仪在线状态，并根据超时时间恢复正常状态
 * @param gyro_str 陀螺仪数据结构体（包含失联计时）
 * @param gyro_state 陀螺仪状态变量
 */ 	
#define gyro_state_trr(gyro_str,gyro_state)\
		if(++(gyro_str->lose_time)>=lose_time_max_gyro){gyro_state=gyro_lose;gyro_str->lose_time=lose_time_max_gyro;}\
		switch(gyro_state){\
			case gyro_ok:break;\
			case gyro_lose:{\
				if(gyro_str->lose_time<=lose_time_min_gyro)\
					gyro_state=gyro_ok;\
			}break;\
		}
/**
 * @brief 系统状态监控任务
 * @details 周期性（1ms）更新各模块状态，包括通信、遥控器、裁判系统、电机等
 * @param argument 任务参数（未使用）
 */ 		
void qwf_state_task(void const * argument)
{
	static TickType_t xLastWakeTime;
    const TickType_t xPeriod = pdMS_TO_TICKS(1); // 1ms周期
    // 初始化唤醒时间
    xLastWakeTime = xTaskGetTickCount();	
	while(1)
	{		
		qwf_state->sys_tick++;
//		if(!ctrl->KM[now].key.r)
//			HAL_IWDG_Refresh(&hiwdg);
		if((qwf_state->sys_tick-qwf_state->communicate.online.tick)>=lose_time_max_communicate){
			qwf_state->communicate.online.yes=0;			
			qwf_state->communicate.state=communicate_lose;
		}
		switch((int)qwf_state->communicate.state){
			case communicate_ok:break;
			case communicate_lose:{
				if(qwf_state->sys_tick-qwf_state->communicate.online.tick<=lose_time_min_communicate){
					qwf_state->communicate.state=communicate_ok;
					qwf_state->communicate.online.yes=1;			
				}
			}break;		
			case communicate_head_error:break;
		}
		if(qwf_state->sys_tick-qwf_state->head.rc.dt7.online.tick>=lose_time_max_RC){
			qwf_state->head.rc.dt7.online.yes=0;			
			qwf_state->head.rc.dt7.state=rc_lose;
		}
		switch((int)qwf_state->head.rc.dt7.state){
			case rc_ok:break;
			case rc_lose:{	
			if(qwf_state->sys_tick-qwf_state->head.rc.dt7.online.tick<=lose_time_min_RC){
				qwf_state->head.rc.dt7.online.yes=1;				
				qwf_state->head.rc.dt7.state=rc_ok;
			}
			}break;		
		}
		if((qwf_state->sys_tick-qwf_state->head.rc.vt13.online.tick)>=lose_time_max_RC){
			qwf_state->head.rc.vt13.online.yes=0;			
			qwf_state->head.rc.vt13.state=rc_lose;
		}
		switch((int)qwf_state->head.rc.vt13.state){
			case rc_ok:break;
			case rc_lose:{	
			if(qwf_state->sys_tick-qwf_state->head.rc.vt13.online.tick<=lose_time_min_RC){
				qwf_state->head.rc.vt13.online.yes=1;							
				qwf_state->head.rc.vt13.state=rc_ok;
			}
			}break;		
		}
		if((qwf_state->sys_tick-qwf_state->chassis.judge.online.tick)>=lose_time_max_judge){
			qwf_state->chassis.judge.online.yes=0;			
			qwf_state->chassis.judge.state=judge_lose;
		}
		switch((int)qwf_state->chassis.judge.state){
			case communicate_ok:break;
			case communicate_lose:{
				if(qwf_state->sys_tick-qwf_state->chassis.judge.online.tick<=lose_time_min_judge){
					qwf_state->chassis.judge.state=judge_ok;
					qwf_state->chassis.judge.online.yes=1;			
				}
			}break;		
			case communicate_head_error:break;
		}
		#if Circuit_Boards!=Chassis_Circuit_Boards		

		if(qwf_state->head.rc.dt7.state==rc_ok){
			ctrl->type=DT7;
		}
		else if(qwf_state->head.rc.vt13.state==rc_ok){
			ctrl->type=VT13;		
		}
		else if(ctrl->mode==KM){
			ctrl->type=Judge;		
		}
		else{
			ctrl->type=Null_Ctrl;
		}
			
		if(qwf_state->sys_tick-qwf_state->head.vision.online.tick>=lose_time_max_vision){
			qwf_state->head.vision.online.yes=0;			
			qwf_state->head.vision.state=vision_lose;
		}
		switch((int)qwf_state->head.vision.state){
			case vision_ok:break;
			case vision_lose:{
				communicate->head.vision.data.str.head_ok=0;
				if(qwf_state->sys_tick-qwf_state->head.vision.online.tick<=lose_time_min_vision){
					qwf_state->head.vision.online.yes=1;			
					qwf_state->head.vision.state=vision_ok;
				}
			}break;		
		} 	
		gm6020_state_trr_func(qwf_state->head.motor.gimbal.yaw	,head->part.gimbal.yaw.motor)
		gm6020_state_trr_func(qwf_state->head.motor.gimbal.pitch,head->part.gimbal.pitch.motor)		
		c620_state_trr_func(qwf_state->head.motor.shoot.fire.l	,head->part.shoot.fire.l);	
		c620_state_trr_func(qwf_state->head.motor.shoot.fire.r	,head->part.shoot.fire.r);		
		c620_state_trr_func(qwf_state->head.motor.shoot.knob	,head->part.shoot.knob);				
		#endif
		#if	Circuit_Boards!=Head_Circuit_Boards

		if(qwf_state->sys_tick-qwf_state->chassis.super_elec.online.tick>=lose_time_max_super_elec){
		qwf_state->chassis.super_elec.state=super_elec_lose;
		}\
		switch((int)qwf_state->chassis.super_elec.state){\
			case super_elec_ok:break;
			case super_elec_lose:{
				super_elec->rx.cap_v=0;
				if(qwf_state->sys_tick-qwf_state->chassis.super_elec.online.tick<=lose_time_max_super_elec)
					qwf_state->chassis.super_elec.state=super_elec_ok;
			}break;
		}
		if(qwf_state->sys_tick-qwf_state->head.motor.gimbal.yaw.online.tick>=lose_time_max_gm6020){
			qwf_state->head.motor.gimbal.yaw.state=gm6020_lose;
		}
		switch((int)qwf_state->head.motor.gimbal.yaw.state){
		case gm6020_ok:break;
		case gm6020_lose:{
			if(qwf_state->sys_tick-qwf_state->head.motor.gimbal.yaw.online.tick<=lose_time_min_gm6020)
				qwf_state->head.motor.gimbal.yaw.state=gm6020_ok;
		}break;
		}
		#if Chassis_Wheel_Type==Helf_Helm_Wheel		
		gm6020_state_trr_func(qwf_state->chassis.motor.f.helm,	chassis->part.f.helm.motor)
		gm6020_state_trr_func(qwf_state->chassis.motor.b.helm,	chassis->part.b.helm.motor)
		
		c620_state_trr_func(qwf_state->chassis.motor.f.wheel,	chassis->part.f.wheel.motor);
		c620_state_trr_func(qwf_state->chassis.motor.b.wheel,	chassis->part.b.wheel.motor);
		c620_state_trr_func(qwf_state->chassis.motor.l.wheel,	chassis->part.l.wheel.motor);
		c620_state_trr_func(qwf_state->chassis.motor.r.wheel,	chassis->part.r.wheel.motor);
		#elif Chassis_Wheel_Type==Leg_Wheel
		
		#endif
		#endif 	
		vTaskDelayUntil(&xLastWakeTime, xPeriod);	
	}
}
